#ifndef RSIM_PRISMATICX_H
#define RSIM_PRISMATICX_H

/* -------------------------------------------------------------------------- *
 * File: PrismaticX.h                                                         *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "Joint.h"
#include "Utils.h"

namespace RSIM{
	
class Rotation;
class ABAData;
class EOMData;	

class PrismaticX:public Joint{
	private:
		/** Create a prismatic joint with origins M and F coincident for default configuration.
		The dof is translation in z direction of both the frames which are coincident. */
		PrismaticX(const int& NodeIndex);
	public:
		/** Create a new Prismatic joint */
		static PrismaticX* New(const int& NodeIndex);
		
		/** Print the joint type */
		void printJointType() const;				
		
		const bool areQuaternionsInUse()const{return false;}
		
		int getDOF()const{return 1;}
		
		void getHColumn(const int& uindex, 
                                double *hptr)const;
		
		void setNQ(int *q)const{q[0]=1;}
		
		void setNU(int *u)const{u[0]=1;}
		
		/** fills *q with default values of q. */
		void setDefaultQ(double *q)const;
		
		/** fills *u with default values of q*/
		void setDefaultU(double *u)const;
		
		/** Update joint Transform */
		void updJointTransform(const double *q,
				       const RigidBodyNode *Parent,
				       const int& pChildID,
				       double *betak);
				       
		void updBetaK(	const double *q,
				const RigidBodyNode *Parent,
				const int& pChildID,
				double *betak)const;
		
		void updRelVelocity(const State& state, Vect3& w, Vect3& v)const;
		
		void updVelocity(const State& state, 
				 const RigidBodyNode *Parent, 
				 const Rotation& prF_C_F, 
				 const double *betak,
				 Vect3& w,
				 Vect3& v) const ;
			
		int getJointDOF()const;
		
		void calc_H_x_UDot(const State& state, double *h_x_udot) const;
		
		void calcQDot(State& state)const;
		
		void calcAKT(const RigidBodyNode *Parent, 
			     const RigidBodyNode *Body,
			     const int& ChildID,
			     const double *betak,
			     State& state,
			     Vect6& sAkt) const;
		
		void sweepForward(const RigidBodyNode *Parent,
				  const int& ChildID,
				  const Vect6& sAkt,
				  const Rotation& FCF,
				  const ABAData *aba,
				  const double *betak,
				  EOMData *eom,
				  State& state
				  );
		
		void sweepBackwards(const bool& isLeaf, 
				    const int& ChildID,
				    const Vect6& sAkt,
				    const Rotation& FCF,					    
				    const EOMData *eom,
				    const double *betak,
				    RigidBodyNode *Parent,
				    ABAData *aba) const;
}; // class Prismatic

inline void PrismaticX::getHColumn(const int& uindex, 
                                  double *hptr)const{
        #ifdef RSIM_SAFE
        Utils::rsim_assert(uindex==0,"Joint::PrismaticX::getHColumn(), uindex must be equal to 0");        
        #endif
        
        hptr[0] = 0.0;  hptr[1] = 0.0;  hptr[2] = 0.0;
        hptr[3] = 1.0;  hptr[4] = 0.0;  hptr[5] = 0.0;
}

}
#endif